/**
 * @file bucket_transport_task.h
 * @brief 实现条筒抓取与放置任务和与调度系统进行协作
 * @author weeksun
 * @version 1.0
 * @date 2017-09-30
 */
#ifndef BUCKET_TRANSPORT_TASK_H
#define BUCKET_TRANSPORT_TASK_H

#include "robot_driver_base.h"
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/LinearMath/Vector3.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf/LinearMath/Transform.h>
#include <tf/LinearMath/Matrix3x3.h>
#include <geometry_msgs/PoseArray.h>
#include <std_msgs/Int32.h>
#include <math.h>
#include <visualization_msgs/Marker.h>
#include <iostream>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <tf/transform_listener.h>

namespace AgvAssistant {

class BucketTransport;
typedef boost::shared_ptr<BucketTransport> BucketTransportPtr;   ///< BucketTransport共享指针类型

/**
 * @brief 工作区域管理
 */
class AreaManager{
public:
  friend class BucketTransport;
  /**
   * @brief 区域类型
   */
  enum AreaType{
    AREA_GET,    ///< 抓取区域
    AREA_PUT     ///< 放置区域
  };
  /**
   * @brief 热区类型
   */
  enum HotAreaType{
    EXIST_TARGET_OBJECT,    ///< 存在目标物体
    NO_EXIST_TARGET_OBJECT,  ///< 不存在目标物体
    OBSTACLE_OCCUPIED        ///< 障碍物占据
  };

  /**
   * @brief 热区
   */
  struct HotArea{
    tf::Vector3 hot_point;   ///< 热点
    HotAreaType type; ///< 热区类型
  };

  /**
   * @brief 区域网格类型
   */
  struct AreaGrid{
    visualization_msgs::Marker points;
    visualization_msgs::Marker line_strip;
    visualization_msgs::Marker line_list;
  };

  /**
   * @brief 创建区域管理器
   * @param hot_area_len [in] 热区划分间隔，热区间隔仅在抓取区域有效(单位m)
   * @param target_object_radius [in] 目标物体半径，多用于放置区域设置热区大小(单位m)
   * @param distance_of_expandsion [in] 区域膨胀距离，用于检测机器人是否接近区域边界(单位m)
   */
  AreaManager(float hot_area_len = 0.8,float target_object_radius = 0.25,
              float distance_of_expandsion = 1.5);
  /**
   * @brief 生成区域信息
   * @param area_seq [in] 区域编号
   * @param area_type [in] 区域类型
   * @param points [in] 区域的四个定点，区域为矩形，区域X轴方向由索引0指向索引1
   * @param dock [in] 工作区域停靠定位点，工作区初始定位点
   * @return 成功生成返回true
   */
  bool generateArea(int area_seq,AreaType area_type,
                    const std::vector<tf::Vector3>& points,
                    const tf::Vector3 dock);
  /**
   * @brief 输出rviz可视化信息
   * @param area_visual_ [out] 可视化信息
   */
  void areaVisualization(AreaGrid& area_visual_);

  /**
   * @brief 检测给定Pose是否接近区域边界
   * @param pose [in] 机器人位姿
   * @return 接近返回true
   */
  bool isCloseBorder(const tf::Transform &pose);
  /**
   * @brief 获取定位点位姿
   * @return 定位点位姿
   */
  tf::Transform getDockGlobalPose();
  /**
   * @brief 获取定位点位姿
   * @param HotArea hot_area [in] 热区信息
   * @return 定位点位姿
   */
  tf::Transform getPositionGlobalPose(HotArea hot_area);
  /**
   * @brief 机器人是否在定位点，确保位姿正确，方向正对工作区,位置只计算Y方向偏差
   * @param pose [in|out] 输入机器人当前位姿，若机器人不再定位点输出调整偏移量
   * @return 机器人在定位点返回true
   */
  bool isRobotInDock(tf::Transform& pose);
  /**
   * @brief 机器人是否在定位点，确保位姿正确，方向正对工作区,位置只计算Y方向偏差
   * @param pose [in] 热区信息
   * @param pose [in|out] 输入机器人当前位姿，若机器人不再定位点输出调整偏移量
   * @return 机器人在定位点返回true
   */
  bool isRobotInPosition(HotArea hot_area,tf::Transform &pose);
  /**
   * @brief 检查条筒预选位置是否符合要求，条筒是否处于工作区内，在工作区内标记识别到的条筒位置
   * @param bucket_points [in] 包含若干条筒位置信息的向量
   * @return 存在满足要求的条筒则返回条筒位置的索引，否则返回-1
   * @author weeksun
   */
  int checkBucketPosition(std::vector<tf::Vector3>& bucket_points);
  /**
   * @brief 检查热区内是否有条筒
   * @param hot_area [in] 热区信息
   * @param bucket_points [in] 包含若干条筒位置信息的向量
   * @return 热区内有条筒,指向下一个热区
   */
  bool BucketInHotArea(HotArea& hot_area,std::vector<tf::Vector3> bucket_points);

  /**
   * @brief 检测热区内是否有障碍
   * @param hot_area [in] 热区信息
   * @return 热区内有障碍，指向下一个热区
   */
  //bool ObstacleInHotArea(HotArea& hot_area,BucketTransport bucketransport);

private:
  int area_seq_ = 0;   ///< 区域编号
  AreaType area_type_ = AREA_GET;  ///< 区域类型
  std::vector<tf::Vector3> area_points_; ///< 区域连接点
  tf::Vector3 dock_;                     ///< 工作区初始定位点,全局坐标
  tf::Transform area2world_;     ///< 区域坐标系在世界坐标系下的变换
  std::vector<HotArea> hot_areas_;  ///< 热区，用于指示机器人巡航路径
  std::vector<int> hot_area_num_;    ///< 热区顺序索引
  int hot_area_n_;    ///< 当前热区编号
  int last_hot_area_ind_ = 0;           ///< 最后一次抓取所在的热区索引
  int last_hot_area_ind_x_ = 0;         ///< 最后一次抓取所在的热区X轴索引
  int last_hot_area_ind_y_ = 0;         ///< 最后一次抓取所在的热去Y轴索引
  float distance_of_expandsion_ = 1.5;  ///< 区域膨胀距离，用于检测机器人是否接近区域边界
  float hot_area_len_;      ///< 热区划分间隔(单位m)
  float target_object_radius_; ///< 目标物体半径
  float real_x_slice_;         ///< 实际热区X方向长度(单位m)
  float real_y_slice_;         ///< 实际热区Y方向长度(单位m)
  int real_x_size_;            ///< 实际热区X方向分片数量
  int real_y_size_;            ///< 实际热区Y方向分片数量

};



/**
 * @brief 该类包含执行条筒抓取与放置任务的代码
 */
class BucketTransport
{
public:

  /**
   * @brief 任务请求反馈信息
   */
  enum TaskReqStatus{
    TASK_REQ_SUCCESS,      ///< 任务请求成功
    AREA_NO_EXIST,         ///< 指定区域不存在或在机器人周围搜索不到区域
    TASK_EXECUTING         ///< 任务执行中

  };



  /**
   * @brief 创建条筒搬运任务对象
   * @param driver [in] 机器人驱动
   */
  BucketTransport(RobotDriverBasePtr& driver,std::string name);
  ~BucketTransport();

  void getParam();

  void CamDetectionInfoCallback(const boost::shared_ptr<geometry_msgs::PoseArray>& nearest_point);

//  void CircleCenterPointCallback(const boost::shared_ptr<geometry_msgs::Pose>& circle_center_point);

  /**
   * @brief 条筒检查接受回调
   * @param bucket_poses[in] 世界坐标系下的条筒位姿
   * @author xd
   */
  void BucketDetectionCallback(const boost::shared_ptr<geometry_msgs::PoseArray>& bucket_poses);
  /**
   * @brief 机器人世界坐标接收回调函数
   * @param pose [in] 机器人世界坐标系下的位姿
   */
  void worldPoseCallback(const geometry_msgs::Pose& pose);

  /**
   * @brief 激光数据在机器人坐标系下坐标接收回调函数
   * @param points [in] 机器人坐标系下的激光数据坐标
   */
  void laserPoseCallback(const std::vector<tf::Vector3>& points);


  /**
   * @brief 机器人轮式里程计接收回调
   * @param msg [in] 轮式里程计信息
   */
  void robotOdomCallback(const nav_msgs::OdometryConstPtr& msg);
  /**
   * @brief 位姿调整响应回调函数
   * @param ack [in] 位姿调整响应信息
   */
  void poseAdjustAckCallback(const RobotDriverBase::PoseAckInfo& ack);
  /**
   * @brief 群控回调函数
   * @param ctl [in] 群控信息
   */
  void groupControlCallback(const RobotDriverBase::GroupControlInfo& ctl);
  /**
   * @brief 应急处理回调函数
   * @param info [in] 应急信息
   */
  void emergentCallback(const RobotDriverBase::EmergentInfo& info);
  /**
   * @brief 工作区设置回调函数
   * @param area [in] 工作区
   */
  void workAreaCallback(const WorkArea& area);

  bool ObstacleInHotArea(tf::Vector3 hot_point_world);
//  std::vector<tf::Vector3> getLaserWorldPoints(){
//    std::vector<tf::Vector3> laser_world;
//    for(int i=0;i<points_robot_.size();i++){
//      laser_world.push_back(points_robot_[i]);
//    }
//    return laser_world;
//  }

private:
  /**
   * @brief 任务类型
   */
  enum TaskType{
    NO_TASK,      ///< 无任务
    GET_TASK,     ///< 抓取任务
    PUT_TASK      ///< 放置任务
  };

  /**
   * @brief 抓筒任务所需数据
   */
  struct GetTaskData{
    /**
     * @brief 条筒抓取任务主状态
     */
    enum MainStatus{
      INIT_GET,           ///< 初始化抓取任务
      BUCKET_GET,         ///< 条筒抓取
      FIND_BUCKET,      ///< 位置移动
      GET_SUCCESS,        ///< 抓取成功
      EMERGENCY,          ///< 紧急情况
      AVOID_OBSTACLE,      ///< 避障情况
      POINT_SEARCH
    };

    GetTaskData():
    lock_area_seq(-1),
    main_status(INIT_GET),
   // main_status(BUCKET_GET),
    sub_status(0),//50
    pre_main_status(INIT_GET){;}

    tf::Transform area2robot;
    RobotDriverBase::GroupControlInfo ctl_;
    int lock_area_seq;   ///< 锁定区域，未锁定赋值为-1
    MainStatus main_status;  ///< 抓取任务主状态
    MainStatus pre_main_status;  ///< 先前主状态
    int sub_status;          ///< 子任务状态
    std::vector<tf::Vector3> bucket_points; ///< 待选抓筒点
  };

  struct PutTaskData{
    /**
     * @brief 放筒任务所需数据
     */
    enum MainStatus{
      EMERGENCY,           ///< 紧急情况
      BUCKET_PUT,          ///< 旋转放筒
      MOVE_POSITION,       ///< 位置移动
      PUT_SUCCESS,         ///< 放置成功
      AVOID_OBSTACLE_PUT,   ///< 避障情况
      POINT_PUT
    };
    PutTaskData():
    lock_area_seq(-1),
    main_status(MOVE_POSITION),
    sub_status(0){;}

   // RobotDriverBase::GroupControlInfo ctl_;
    int lock_area_seq;   ///< 锁定区域，未锁定赋值为-1
    MainStatus main_status;  ///< 抓取任务主状态
    int sub_status;          ///< 子任务状态
    float robot_bucket_dis=1.5;   ///<机器人中心和筒心距离

  };

  /**
   * @brief 工作线程主函数
   */
  void working_exec();
  /**
   * @brief 抓筒任务逻辑
   */
  void get_task_exec();
  /**
   * @brief 放筒任务逻辑
   */
  void put_task_exec();
  /**
   * @brief 添加任务作业区域
   * @param area [in] 区域对象
   * @see AreaManager
   * @return 成功添加区域返回true
   */
  bool addArea(const AreaManager& area);

  /**
   * @brief 请求执行抓取任务
   * @param index [in] 抓取区域编号，不指定或设置为-1表示自动搜索
   * @return 返回请求信息
   * @see TaskReqStatus
   */
  TaskReqStatus requestGetTask(RobotDriverBase::GroupControlInfo ctl,int index = -1);
  /**
   * @brief 请求执行放置任务
   * @param index [in] 放置区域编号，不指定或设置为-1表示自动搜索
   * @return 返回请求信息
   * @see TaskReqStatus
   */
  TaskReqStatus requestPutTask(RobotDriverBase::GroupControlInfo ctl,int index = -1);

  /**
   * @brief 执行的抓筒区域是否锁定，所指定的编号不存在则返回未锁定
   * @param index [in] 区域编号
   * @return 区域锁定返回true
   */
  bool isLockGetArea(int index);
  /**
   * @brief 执行的放筒区域是否锁定，所指定的编号不存在则返回未锁定
   * @param index [in] 区域编号
   * @return 区域锁定返回true
   */
  bool isLockPutArea(int index);
  /**
   * @brief 获取机器人当前位姿
   * @return 返回机器人当前位姿
   */
  tf::Transform getGlobalPose();
  /**
   * @brief 获取条筒位置信息
   * @return 返回条筒备选位置
   */
  std::vector<tf::Vector3> getBucketPosition();
  /**
   * @brief 发送位姿调整请求
   * @param pose [in] 目标位姿与当前机器人位姿之间的偏移量，参考系:目标位姿
   * @param sw_x [in] 是否调整x方向，默认调整
   * @param sw_y [in] 是否调整y方向，默认调整
   * @param sw_th [in] 是否调整z轴旋转，默认调整
   * @return 成功请求放回true
   */
  bool sendPoseAdjReq(const tf::Transform& pose,bool sw_x = true,bool sw_y = true,bool sw_th = true);
  /**
   * @brief 发送位姿调整请求
   * @param pose [in] 目标位姿与当前机器人位姿之间的偏移量，参考系:目标位姿
   * @param sw_x [in] 是否调整x方向
   * @param sw_y [in] 是否调整y方向
   * @param sw_th [in] 是否调整z轴旋转
   * @param max_vel_x [in] x方向的调整速度
   * @param max_vel_y [in] y方向的调整速度
   * @return 成功请求放回true
   */
  bool sendPoseAdjReq(const tf::Transform& pose,bool sw_x,bool sw_y ,bool sw_th ,double max_vel_x, double max_vel_y);
  /**
   * @brief 发送位姿调整请求
   * @param pose [in] 目标位姿与当前机器人位姿之间的偏移量，参考系:目标位姿
   * @param sw_x [in] 是否调整x方向
   * @param sw_y [in] 是否调整y方向
   * @param sw_th [in] 是否调整z轴旋转
   * @param max_vel_x [in] x方向的调整速度
   * @param max_vel_y [in] y方向的调整速度
   * @param max_vel_th [in] 旋转速度
   * @return 成功请求放回true
   */
  bool sendPoseAdjReq(const tf::Transform& pose,bool sw_x,bool sw_y ,bool sw_th ,double max_vel_x, double max_vel_y, double max_vel_th);
  /**
   * @brief 获取位姿请求ACK信息
   * @param ack [out] 位姿请求ACK信息
   * @return 接收到ACK返回true
   */
  bool getPoseAdjAck(RobotDriverBase::PoseAckInfo& ack);
  /**
   * @brief 选出距离小车最近的条桶坐标
   * @param have_bucket [out] 是否检测出条桶
   * @param bucket2robot [out] 条桶相对于小车的坐标
   * @param angle [out] 小车到条桶向量相对于世界坐标系x轴夹角
   * @param bucket_pose 条桶在世界坐标系下坐标
   * @return 条桶相对于小车的坐标
   */
  tf::Transform GetBucketPose(bool& have_bucket,tf::Transform& bucket2robot,double& angle,tf::Transform& bucket_pose,int &index);
  /**
   * @brief 计算预捕获点
   * @param bucket2robot [in] 机器人坐标系下条桶坐标
   * @param angle [in] 小车到条桶向量相对于世界坐标系x轴夹角
   * @param bucket_pose [out] 世界坐标系下预捕获点坐标
   */
  void preCapturePoints(tf::Transform bucket2robot,double angle,tf::Transform bucket_pose);
  /**
   * @brief 选出将要前往的预捕获点
   * @param pose [out] 将要前往的预捕获点坐标
   * @return 成功找到返回true
   */
  bool preCaptureSelect(tf::Transform &pose);
  /**
   * @brief 用于显示预捕获点的位置和选择过程
   * @param bucket_pose [in] 世界坐标系下条桶坐标
   * @param precapture2robot_final [in] 世界坐标系下预捕获点坐标
   */
  void preCapture_show(tf::Transform bucket_pose,tf::Transform& precapture2robot_final);

  /**
   * @brief 遍历找桶时计算下一个目标点坐标
   * @param area [in|out] 抓桶区域
   * @param hot_area_num [in] 热区的数量
   * @param hot_areas [out] 存储热区
   * @return 成功找到下一个目标点返回true
   */
  tf::Transform GetTargetPoint(AreaManager& area, std::vector<int> hot_area_num,std::vector<AreaManager::HotArea> hot_areas);
  /**
   * @brief 计算前往的放桶位置坐标
   * @param area [in|out] 放桶区域
   * @param hot_areas [in] 存储热区
   * @return 成功找到放桶点返回true
   */
  tf::Transform PutAreaSearch(AreaManager& area,std::vector<AreaManager::HotArea> hot_areas);

private:
  RobotDriverBasePtr robot_driver_ptr_;   ///< 机器人驱动共享指针对象
  boost::thread working_th_;     ///< 任务线程对象
  ros::NodeHandle nh_private_;  ///< 节点私有句柄
  ros::NodeHandle nh_;  ///< 全局句柄
  ros::NodeHandle nh_bucket_transport_; ///< 条筒搬运任务句柄
  std::string task_name_;    ///< 条筒搬运任务名称，作为图元名称前缀

  float perspective_;        ///< 检测视角

  double max_vel_x_;         ///< X方向最大速度
  double max_vel_y_;         ///< Y方向最大速度
  double max_vel_th_;        ///< Z最大旋转角速度
  double precision_x_;        ///< x方向调整精度
  double precision_y_;        ///< y方向调整精度
  double precision_th_;       ///< z轴旋转调整精度
  float distance_bucket_;     ///< 在距离条桶指定位置停下
  float back_distance_;       ///< 接收到红外后后退距离
  float precapture_angle_;    ///< 预捕获点设置角度
  boost::mutex data_update_mutex_;         ///< 数据更新
  RobotDriverBase::PoseAckInfo pose_ack_;  ///< 位姿调整ACK信息
  bool recv_pose_ack_;                     ///< 是否收到位姿调整ACK
  std::vector<tf::Vector3> bucket_points_;  ///< 条筒预选位置
  std::vector<tf::Vector3> bucket_points_final_;  ///< 条筒预选位置
  float target_radius_; ///< 目标物体半径
  tf::Transform global_pose_;               ///< 机器人全局位姿
  std::vector<tf::Vector3> points_robot_;   ///<世界坐标系下的激光数据坐标(m)
  std::vector<tf::Transform> bucket_precapture_points_; ///<预捕获点世界坐标
  float precapture_radius_;
  tf::Vector3 bucket_nearest_;   ///< 最近条筒的世界坐标


  float avoid_obstacle_width_;  ///<需要避障的宽度(m)
  float avoid_obstacle_distance_;  ///<#需要避障的距离(m)（针对base_link）

  std::map<int,AreaManager> get_area_;  ///< 存储抓筒区域
  std::map<int,AreaManager> put_area_;  ///< 存储放筒区域
  TaskType task_type_ = NO_TASK;        ///< 任务类型
  //TaskType task_type_ = GET_TASK;
  TaskType task_type_pre_ = NO_TASK;    ///< 前一个任务类型
  GetTaskData get_data_;                ///< 抓取任务数据

  PutTaskData put_data_;                ///< 放置任务数据
  ros::Subscriber bucket_sub_;          ///< 订阅条筒检测
  ros::Subscriber camera_sub_;          ///< 订阅相机数据
  ros::Subscriber camera_center_sub_;
  geometry_msgs::Pose nearest_point_;   ///< 相机检测到最近点坐标
  geometry_msgs::Pose circle_center_point_; ///< 相机检测到桶心坐标


  bool emergent_info_=false;          ///< 紧急消息请求
  bool reset_putarea_ = true;            ///< 重设区域请求
  bool reset_getarea_ = true;            ///< 重设区域请求

  tf::Transform pose_;
  tf::Transform straight2pre_;       ///< 选出的预捕获点坐标
  tf::Transform precapture2robot_final_;   /// 世界坐标系下预捕获点坐标
  tf::Transform bucket_pose_;       ///< 世界坐标系下条桶坐标

  float laser2baselink_;        ///< baselink坐标系下激光数据坐标
  std::vector<float> nearest_points_;   ///< 相机检测中最近点坐标
  float camera2bucket_dis_;         ///< 相机相对条桶距离

  ros::Publisher  camera_control_pub_;

  std_msgs::Int32 camera_control_;

  unsigned char data_recv_[4];
  int case_info_=0;

  RobotDriverBase::OrderClass order_;

  float bucketPut_distance_;
  float bucket_radius_;
  RobotDriverBase::GroupControlInfo ctl_;

  bool stop_adjust_=true;
  bool point_search_type_ = false;
  tf::Vector3 get_point_;
  tf::Vector3 put_point_;
  double angle_;
  float point_back_dis_;

  ros::Publisher laser_detection_reset_pub_;
  std_msgs::Int32 laser_detection_reset_;
  int camera_adjust_;
  tf::Vector3 p1_;
  tf::Vector3 p3_;
};
}


#endif // BUCKET_TRANSPORT_TASK_H
